#include "hx712.h"
#include <vector>
#include <algorithm>

uint32_t HX712::read()
{
    uint32_t Count;
    uint8_t i;
    digitalWrite(hx_sck,0);
    delayMicroseconds(1);
    Count=0;
    while(digitalRead(hx_dio));
    for (i=0;i<26;i++)
    {
        digitalWrite(hx_sck,1);
        delayMicroseconds(1);
        Count=Count<<1;
        digitalWrite(hx_sck,0);
        delayMicroseconds(1);
        if(digitalRead(hx_dio)) Count++;
    }

    digitalWrite(hx_sck,1);
    delayMicroseconds(1);
    Count=Count^0x800000;
    digitalWrite(hx_sck,0);
    delayMicroseconds(1);
	
    return Count;
}

long HX712::readMulti(int num)
{
    std::vector<long> ver;
    long cnt = 0;

    for (size_t i = 0; i < 10; i++){
        ver.push_back(read());
    }
    std::sort(ver.begin(),ver.end());
    for (size_t i = 2; i < num-2; i++){
        cnt+=ver.at(i);
    }
    ver.clear();
    return cnt/(num-4);
}


int HX712::readReal()
{
    long reading = readMulti();
    float val = gain*(reading-offset)/1000;
    float n;
    
    if(std::abs(val-stable) >= 1){
        n_1 = val;
        stable = val;
        isStable = false;
    }else{
        n = smooth*val+(1-smooth)*n_1;
        n_1 = n;
        isStable = true;
    }

    return std::round(stable);
}
